#include "DrivableSeg.h"

#include "iomanip"
namespace ObstacleCluster {
    DrivableSeg::DrivableSeg(ros::NodeHandle &nh) {

#ifdef hesai
        sub_lidarcloud = nh.subscribe<sensor_msgs::PointCloud2>(_lidar_topic,1,&DrivableSeg::lidarCallback, this);
#endif

#ifdef zvision
        sub_lidarcloud = nh.subscribe<sensor_msgs::PointCloud2>(zvision_lidar_topic,1,&DrivableSeg::lidarCallback, this);
#endif
        pub_segmented_obj = nh.advertise<sensor_msgs::PointCloud2>(_SegObjPub_topic,1);
        pub_ground = nh.advertise<sensor_msgs::PointCloud2>(_GroundPub_topic,1);
        pub_reversed_zvision_lidar = nh.advertise<sensor_msgs::PointCloud2>("reversed_zvision_lidar",1);

        gpfmethod = std::make_shared<GroundPlaneFit>();

        row_angle_sin.clear();
        row_angle_cos.clear();

#ifdef hesai
        float step_row = (float) _ang_res_y / 180 * M_PI;
        float start_row = ((float) _ang_bottom / 180 * M_PI);
        for (int i = 0; i < _N_SCAN; i++) {
            row_angle_sin.push_back(sin(start_row));
            row_angle_cos.push_back(cos(start_row));
            start_row -= step_row;
        }
#endif
#ifdef zvision
        float step_row = (float) _ang_res_y / 180 * M_PI;
        float start_row = ((float) vertical_bottom / 180 * M_PI);
        for (int i = 0; i < _N_SCAN; i++) {
            row_angle_sin.push_back(sin(start_row));
            row_angle_cos.push_back(cos(start_row));
            start_row -= step_row;
        }
#endif
        threshold = _DRIVABLE_ANGLE_TH * M_PI / 180;

        x_mat = cv::Mat::zeros(_N_SCAN, _Horizon_SCAN, CV_32F);
        y_mat = cv::Mat::zeros(_N_SCAN, _Horizon_SCAN, CV_32F);

        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
        nanPoint.intensity = -1;

        fullPointCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
        SegObjectCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
        GroundCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
        ProjectedPointCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());

        ProjectedPointCloud->width = _N_SCAN * _Horizon_SCAN;
        ProjectedPointCloud->height = 1;
        ProjectedPointCloud->points.resize(_N_SCAN * _Horizon_SCAN);

        boundary_box_filter_.setMin(Eigen::Vector4f(-20, -20, -CAR_HEIGHT-0.2, 1.0));
        boundary_box_filter_.setMax(Eigen::Vector4f(20, 20, 4-CAR_HEIGHT, 1.0));
        Outlier_box_filter_.setMeanK(30);
        Outlier_box_filter_.setStddevMulThresh(1.0);

        Voxel_box_filter_.setLeafSize(0.05f,0.05f,0.05f);

        std::pair <int8_t, int8_t> neighbor;
        neighbor.first = -1;
        neighbor.second = 0;
        neighborIterator.push_back(neighbor);
        neighbor.first = 0;
        neighbor.second = 1;
        neighborIterator.push_back(neighbor);
        neighbor.first = 0;
        neighbor.second = -1;
        neighborIterator.push_back(neighbor);
        neighbor.first = 1;
        neighbor.second = 0;
        neighborIterator.push_back(neighbor);

        allPushedIndX = new uint16_t[_N_SCAN * _Horizon_SCAN];
        allPushedIndY = new uint16_t[_N_SCAN * _Horizon_SCAN];

        queueIndX = new uint16_t[_N_SCAN * _Horizon_SCAN];
        queueIndY = new uint16_t[_N_SCAN * _Horizon_SCAN];

    }

//计算Scan之间点云的角度
    void DrivableSeg::ComputeScanAngle(const cv::Mat &range_mat) {
        // todo 可以考虑一下角度图在时间上的关系

        angle_image = cv::Mat(_N_SCAN, _Horizon_SCAN, CV_32F, cv::Scalar::all(0.5 * M_PI));//默认角度为90度
        x_mat.row(0) = range_mat.row(0) * row_angle_cos[0];
        y_mat.row(0) = range_mat.row(0) * row_angle_sin[0];
        for (int r = 1; r < _N_SCAN; r++) {
            x_mat.row(r) = range_mat.row(r) * row_angle_cos[r];
            y_mat.row(r) = range_mat.row(r) * row_angle_sin[r];
            for (int c = 0; c < _Horizon_SCAN; c++) {
//            应对激光雷达最下面那3线是没有发射出来，被挡住的情况
//            if((range_mat.at<float>(r,c) > 1000 || range_mat.at<float>(r-1,c)> 1000 ) && r > 12){
//                angle_image.at<float>(r, c) = 0;
//                continue;
//            }
                dx = x_mat.at<float>(r - 1, c) - x_mat.at<float>(r, c);
                dy = fabs(y_mat.at<float>(r - 1, c) - y_mat.at<float>(r, c));

//          处理夹角大于90度的情况. 假设小车不能跨过0.3m的高度 todo 记得再修改回来
//            if(dx < -_Mindx || dy > 0.3) {
//                angle_image.at<float>(r, c) = 0.5 * M_PI;
//                continue;
//            }
                dx = fabs(dx);

                angle_image.at<float>(r, c) = atan2(dy, dx)*180/M_PI;

            }
        }
        for(int j = 0;j<_N_SCAN;j++){
            LOG(INFO)<<"angle_image:"<<j<<" "<<angle_image.row(j);
        }
        LOG(INFO)<<"----------------";


    }


    void DrivableSeg::ComputeDrivableArea(const cv::Mat &range_mat,
                                          pcl::PointCloud<PointType>::Ptr ground_cloud) {

        ComputeScanAngle(range_mat);
        RangeToPointCloud(range_mat);
        Filter(angle_image);
        Label_drivable_area(*fullPointCloud, *ground_cloud, groundMat);

    }

    void DrivableSeg::ComputeDrivableArea(pcl::PointCloud<PointType>::ConstPtr laserCloudIn,
                                          pcl::PointCloud<PointType>::Ptr ground_cloud) {

        ComputeRangeMat(laserCloudIn, rangeMat);  //scan点云转化为深度图
        ComputeScanAngle(rangeMat);  //计算Scan之间点云的水平角度
//        ApplySavitskyGolaySmoothing(angle_image,5);
//        Filter(angle_image);

        Label_drivable_area(*fullPointCloud, *ground_cloud, groundMat);


//        gpf_label_drivabel_area(ground_cloud);

        if(ground_cloud->points.size() == 0)return;

        extract_object();

    }

    void DrivableSeg::ComputeDrivableArea(const cv::Mat &range_mat,
                                          pcl::PointCloud<PointType>::Ptr InputfullCloud,
                                          pcl::PointCloud<PointType>::Ptr ground_cloud, cv::Mat &ground_mat) {
// todo 选择合适的滤波方法

/*    cv::blur(angle_image,angle_image,cv::Size(3,1));
    cv::medianBlur(angle_image,angle_image,3);
    ApplySavitskyGolaySmoothing(angle_image,5);
    cv::GaussianBlur(angle_image,angle_image,cv::Size(3,3),1);*/

        ComputeScanAngle(range_mat);
        Filter(angle_image);
        Label_drivable_area(*InputfullCloud, *ground_cloud, ground_mat);

    }
    // todo 记得这里的输入参数改成指针
    void DrivableSeg::ComputeRangeMat(const pcl::PointCloud <PointType>::ConstPtr InputCloud, cv::Mat &range_mat) {
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize;
        PointType thisPoint;

        range_mat = cv::Mat(_N_SCAN, _Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
        cv::Mat flag_mat = cv::Mat(_N_SCAN, _Horizon_SCAN, CV_8S, cv::Scalar::all(0));
        cv::Mat verticalAngle_mat = cv::Mat(_N_SCAN, _Horizon_SCAN, CV_8S, cv::Scalar::all(-1));
        cloudSize = InputCloud->points.size();
        std::fill(ProjectedPointCloud->points.begin(), ProjectedPointCloud->points.end(), nanPoint);

        for (size_t i = 0; i < cloudSize; ++i) {

#ifdef zvision
            //use reverse
            thisPoint.x = InputCloud->points[i].x;
            thisPoint.y = InputCloud->points[i].y;
            thisPoint.z = InputCloud->points[i].z;

            verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
            rowIdn = (_N_SCAN - 1) - (verticalAngle + _ang_bottom) / _ang_res_y;
            double temp = (verticalAngle + _ang_bottom) / _ang_res_y;

            if (rowIdn < 0 || rowIdn >= _N_SCAN)
                continue;

            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

//        -60~55 ->35~150
            columnIdn = (horizonAngle + 90 - horizon_bottom) / _ang_res_x;

            if (columnIdn < 0 || columnIdn >= _Horizon_SCAN)
                continue;

            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            if (range < sensorMinimumRange)
                continue;

            if(flag_mat.at<int8_t>(rowIdn,columnIdn) == 1){
                if(rowIdn == 0 || columnIdn == 0 || rowIdn == _N_SCAN - 1 || columnIdn == _Horizon_SCAN -1)
                    break;
                float verticalAngle_up,verticalAngle_down;
                while(1){
                    verticalAngle_up = verticalAngle_mat.at<float>(rowIdn+1,columnIdn);
                    verticalAngle_down = verticalAngle_mat.at<float>(rowIdn+1,columnIdn);

                }
            }



            rangeMat.at<float>(rowIdn, columnIdn) = range;

            thisPoint.intensity = (float) rowIdn + (float) columnIdn / 10000.0;

            index = columnIdn + rowIdn * _Horizon_SCAN;
            ProjectedPointCloud->points[index] = thisPoint;
            ProjectedPointCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
            flag_mat.at<int8_t>(rowIdn,columnIdn) += 1;
            verticalAngle_mat.at<float>(rowIdn,columnIdn) = verticalAngle;
#endif
#ifdef hesai
            thisPoint.x = InputCloud.points[i].x;
            thisPoint.y = InputCloud.points[i].y;
            thisPoint.z = InputCloud.points[i].z;

            verticalAngle =atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
            rowIdn = (_N_SCAN - 1) - (verticalAngle + _ang_bottom) / _ang_res_y;

            if (rowIdn < 0 || rowIdn >= _N_SCAN)
                continue;

            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            columnIdn = -round((horizonAngle - 90.0) / _ang_res_x) + _Horizon_SCAN / 2;
            if (columnIdn >= _Horizon_SCAN)
                columnIdn -= _Horizon_SCAN;

            if (columnIdn < 0 || columnIdn >= _Horizon_SCAN)
                continue;

            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            if (range < _sensorMinimumRange)
                continue;

            range_mat.at<float>(rowIdn, columnIdn) = range;

            index = rowIdn * _Horizon_SCAN + columnIdn;
            thisPoint.intensity = range;
            ProjectedPointCloud->points[index] = thisPoint;
#endif

        }

        for(int j = 0;j<_N_SCAN;j++){
            LOG(INFO)<<"flag_mat:"<<j<<" "<<flag_mat.row(j);
        }

    }

    void DrivableSeg::RangeToPointCloud(const cv::Mat &range_mat) {

        float verticalAngle, horizonAngle;
        pcl::PointXYZI thisPoint;
        float tempXYrange;
        for (int rowIdn = 0; rowIdn < _N_SCAN; rowIdn++)
            for (int columnIdn = 0; columnIdn < _Horizon_SCAN; columnIdn++) {
                if (range_mat.at<float>(rowIdn, columnIdn) == FLT_MAX)//如果该点深度不合法
                    continue;
                verticalAngle = ((_N_SCAN - 1 - rowIdn) * _ang_res_y - _ang_bottom) * M_PI / 180;
                horizonAngle = (-(columnIdn - _Horizon_SCAN / 2) * _ang_res_x + 90) * M_PI / 180;

                thisPoint.z = range_mat.at<float>(rowIdn, columnIdn) * sin(verticalAngle);

                tempXYrange = range_mat.at<float>(rowIdn, columnIdn) * cos(verticalAngle);

                thisPoint.y = tempXYrange * cos(horizonAngle);
                thisPoint.x = tempXYrange * sin(horizonAngle);

                thisPoint.intensity = range_mat.at<float>(rowIdn, columnIdn);
                fullPointCloud->points[rowIdn * _Horizon_SCAN + columnIdn] = thisPoint;

            }
    }

    void DrivableSeg::Label_drivable_area(pcl::PointCloud <pcl::PointXYZI> &fullCloud,
                                          pcl::PointCloud <PointType> &ground_cloud, cv::Mat &ground_mat) {

        temp_ground_Mat = cv::Mat::zeros(_N_SCAN, _Horizon_SCAN, CV_8S);
        ground_cloud.clear();
        ground_mat = cv::Mat::zeros(_N_SCAN, _Horizon_SCAN, CV_8S);


/*        int r, c;
        int round_c;

        for (c = 0; c < _Horizon_SCAN; ++c) {
            for (r = _N_SCAN - 1; r >= _ScanEndRow; r--) {
//                if (fullCloud.points[r * _Horizon_SCAN + c].intensity != -1) {//合法点判断
                    //角度满足阈值要求
                    if (angle_image.at<float>(r, c) > threshold )
                        continue;
                    //将合法的、并满足角度要求的点标记在temp_ground_Mat中
                    temp_ground_Mat.at<int8_t>(r, c) = 1;
//                    continue;
//                }
                //不合法的点直接在ground_mat中标记为 -1
//                ground_mat.at<int8_t>(r, c) = -1;
            }
        }

        cv::Mat kernel = getStructuringElement(cv::MORPH_RECT,cv::Size(5,5),cv::Point(-1,-1));
        cv::morphologyEx(temp_ground_Mat, temp_ground_Mat, cv::MORPH_CLOSE, kernel);

        for (c = 1; c < _Horizon_SCAN; c++)
            for (r = _N_SCAN - 1; r >= _ScanEndRow; r--) {
//                if (fullCloud.points[r * _Horizon_SCAN + c].intensity == -1){
//                    ground_mat.at<int8_t>(r, c) = -1;
//                    continue;
//                }
                if (temp_ground_Mat.at<int8_t>(r, c) == 1) {
                    ground_mat.at<int8_t>(r, c) = 1;
                    ProjectedPointCloud->points[r * _Horizon_SCAN + round_c].intensity = angle_image.at<float>(r, c);
                    ground_cloud.push_back(ProjectedPointCloud->points[r * _Horizon_SCAN + c]);
                    continue;
                }
                break;
            }*/


        int r, c;
        int round_c;

        for (c = 0; c < _Horizon_SCAN; ++c) {
            for (r = _N_SCAN - 1; r >= _ScanEndRow; r--) {
                if (fullCloud.points[r * _Horizon_SCAN + c].intensity != -1) {//合法点判断
                    //角度满足阈值要求
                    if (angle_image.at<float>(r, c) > threshold )
                        continue;
                    //将合法的、并满足角度要求的点标记在temp_ground_Mat中
                    temp_ground_Mat.at<int8_t>(r, c) = 1;
                    continue;
                }
                //不合法的点直接在ground_mat中标记为 -1
                ground_mat.at<int8_t>(r, c) = -1;
            }
        }

        for(int j =0;j<_N_SCAN;j++){
            LOG(INFO)<<"temp_ground_mat:"<<j<<" "<<temp_ground_Mat.row(j);
        }
        LOG(INFO)<<"----------------";

        for (c = 1; c < _Horizon_SCAN; c++)
            for (r = _N_SCAN - 1; r >= _ScanEndRow; r--) {
                if (temp_ground_Mat.at<int8_t>(r, c) == 1) {
                    ground_mat.at<int8_t>(r, c) = 1;
                    ProjectedPointCloud->points[r * _Horizon_SCAN + round_c].intensity = angle_image.at<float>(r, c);
                    ground_cloud.push_back(ProjectedPointCloud->points[r * _Horizon_SCAN + c]);
                    continue;
                }
                break;
            }

        ////标记方法一：简单快速
/*        int r, c;
        int round_c;

        for (c = 1; c < _Horizon_SCAN; ++c) {
            for (r = _N_SCAN - 1; r >= _ScanEndRow; r--) {
                if (fullCloud.points[r * _Horizon_SCAN + c].intensity != -1) {//合法点判断
                    //角度满足阈值要求
                    if (angle_image.at<float>(r, c) > threshold)
                        continue;
                    //将合法的、并满足角度要求的点标记在temp_ground_Mat中
                    temp_ground_Mat.at<int8_t>(r, c) = 1;
                    continue;
                }
                //不合法的点直接在ground_mat中标记为 -1
                ground_mat.at<int8_t>(r, c) = -1;
            }
        }

        for (c = 0; c < _Horizon_SCAN; c++)
            for (r = _N_SCAN - 1; r >= _ScanEndRow; r--) {
                if (temp_ground_Mat.at<int8_t>(r, c) == 1) {
                    ground_mat.at<int8_t>(r, c) = 1;
                    fullCloud.points[r * _Horizon_SCAN + round_c].intensity = angle_image.at<float>(r, c);
                    ground_cloud.push_back(fullCloud.points[r * _Horizon_SCAN + c]);
                    continue;
                }
                break;
            }*/

    ////标记方法一

    ////标记方法二：另外一种标记可行驶区域的方法，在楼梯提取上略差，但能去除一些杂七杂八的点,要用的话需要把上面
/*    int NumCnt;
    int c_width = (_DRIVABLE_WIDTH_ANGLE_TH/360)*_Horizon_SCAN;
    int r,c;
    int round_c;

    for (c = 1; c < _Horizon_SCAN; ++c) {
        for (r = _N_SCAN -1; r >= _ScanEndRow; r--) {
            if(fullCloud.points[r*_Horizon_SCAN+c].intensity != -1) {//合法点判断
                //角度满足阈值要求
                if (angle_image.at<float>(r, c) > threshold )
                    continue;
                //将合法的、并满足角度要求的点标记在temp_ground_Mat中
                temp_ground_Mat.at<int8_t>(r, c) = 1;

                continue;
            }
            //不合法的点直接在ground_mat中标记为 -1
            ground_mat.at<int8_t>(r, c) = -1;
        }
    }
//    根据temp_ground_Mat在ground_mat中标记可行驶区域线段，并将对应点云压入ground_cloud
    for(r = _N_SCAN -1;r>=_ScanEndRow;r--){
        for (c = 0; c < _Horizon_SCAN; ++c) {
            round_c = (c+c_width)%_Horizon_SCAN;
            if(r ==_N_SCAN-1) {//如果是最下面一行，_N_SCAN-1行
                //当前线段的首末两个端点是否满足条件
                if (temp_ground_Mat.at<int8_t>(r, c) != 1 || temp_ground_Mat.at<int8_t>(r, round_c) != 1)
                    continue;
                //统计当前线段中的所有点是否满足条件
                for (NumCnt = 0; NumCnt <= c_width; NumCnt++) {
                    round_c = (c + NumCnt) % _Horizon_SCAN;
                    if (temp_ground_Mat.at<int8_t>(r, round_c) != 1)
                        break;
                }
            }
            else{//如果不是最下面一行
                //第r行、第r+1行当前线段的首末两个端点是否满足条件(如果第r行中的线段要为可行驶区域，那第r+1行必须是可行驶区域)
                if(temp_ground_Mat.at<int8_t>(r,c)!=1 || temp_ground_Mat.at<int8_t>(r,round_c)!=1
                   || ground_mat.at<int8_t>(r+1,c)!=1 || ground_mat.at<int8_t>(r+1,round_c)!=1)
                    continue;
                //统计第r行、第r+1行当前线段中的所有点是否满足条件
                for(NumCnt = 0;NumCnt<=c_width;NumCnt++){
                    round_c = (c+NumCnt)%_Horizon_SCAN;
                    if(temp_ground_Mat.at<int8_t>(r,round_c)!=1 ||ground_mat.at<int8_t>(r+1,round_c)!=1)
                        break;
                }
            }
            //如果线段中所有点满足条件，则在ground_mat中标记，ground_cloud中压入对应点
            if(NumCnt >= c_width + 1){
                for(NumCnt = 0;NumCnt<=c_width;NumCnt++){
                    round_c = (c+NumCnt)%_Horizon_SCAN;
                    ground_mat.at<int8_t>(r,round_c) = 1;
                    fullCloud.points[r * _Horizon_SCAN + round_c].intensity = angle_image.at<float>(r,round_c);
                    ground_cloud.push_back(fullCloud.points[r * _Horizon_SCAN + round_c]);
                }
                //在当前行标记完当前线段后，跳过已经被标记了的点
                if(c+NumCnt >_Horizon_SCAN)
                    break;
                else
                    c = c +NumCnt;
            }
        }
    }*/
    ////标记方法二

    }


    cv::Mat DrivableSeg::GetAngleImage() {
        return angle_image.clone();
    }

    cv::Mat DrivableSeg::GetRangeMat() {
        return rangeMat.clone();
    }

    cv::Mat DrivableSeg::GetGroundMat() {
        return groundMat.clone();
    }

    void DrivableSeg::ApplySavitskyGolaySmoothing(cv::Mat &angle_image, int window_size) {


        cv::Mat kernel = GetSavitskyGolayKernel(window_size);
        cv::Mat smoothed_image;  // init an empty smoothed image
        const cv::Point ANCHOR_CENTER = cv::Point(-1, -1);
        const int SAME_OUTPUT_TYPE = -1;

        cv::filter2D(angle_image, angle_image, SAME_OUTPUT_TYPE, kernel, ANCHOR_CENTER,
                     0, cv::BORDER_REFLECT101);

    }

    cv::Mat DrivableSeg::ApplyUniformSmoothing(cv::Mat angle_image, int window_size) {


        cv::Mat kernel2 = GetUniformKernel(window_size);
        cv::Mat smoothed_image;  // init an empty smoothed image
        const cv::Point ANCHOR_CENTER = cv::Point(-1, -1);
        const int SAME_OUTPUT_TYPE = -1;
        cv::filter2D(angle_image, smoothed_image, SAME_OUTPUT_TYPE, kernel2, ANCHOR_CENTER,
                     0, cv::BORDER_REFLECT101);
        return smoothed_image;
    }

    cv::Mat DrivableSeg::GetSavitskyGolayKernel(int window_size) const {
        using cv::Mat;
        if (window_size % 2 == 0) {
            throw std::logic_error("only odd window size allowed");
        }
        bool window_size_ok = window_size == 3 || window_size == 5 || window_size == 7 ||
                              window_size == 9 || window_size == 11;
        if (!window_size_ok) {
            throw std::logic_error("bad window size");
        }
        // below are no magic constants. See Savitsky-golay filter.
        cv::Mat kernel;
        switch (window_size) {
            case 3:
                kernel = Mat::zeros(1, window_size, CV_32F);
                kernel.at<float>(0, 0) = 1.0f;
                kernel.at<float>(0, 1) = 0.0f;
                kernel.at<float>(0, 2) = 1.0f;
                kernel /= 2.0f;
                return kernel;
            case 5:
                kernel = Mat::zeros(window_size, 1, CV_32F);
                kernel.at<float>(0, 0) = -3.0f;
                kernel.at<float>(0, 1) = 12.0f;
                kernel.at<float>(0, 2) = 17.0f;
                kernel.at<float>(0, 3) = 12.0f;
                kernel.at<float>(0, 4) = -3.0f;

                kernel /= 35.0f;
                return kernel;
            case 7:
                kernel = Mat::zeros(window_size, 1, CV_32F);
                kernel.at<float>(0, 0) = -2.0f;
                kernel.at<float>(0, 1) = 3.0f;
                kernel.at<float>(0, 2) = 6.0f;
                kernel.at<float>(0, 3) = 7.0f;
                kernel.at<float>(0, 4) = 6.0f;
                kernel.at<float>(0, 5) = 3.0f;
                kernel.at<float>(0, 6) = -2.0f;
                kernel /= 21.0f;
                return kernel;
            case 9:
                kernel = Mat::zeros(window_size, 1, CV_32F);
                kernel.at<float>(0, 0) = -21.0f;
                kernel.at<float>(0, 1) = 14.0f;
                kernel.at<float>(0, 2) = 39.0f;
                kernel.at<float>(0, 3) = 54.0f;
                kernel.at<float>(0, 4) = 59.0f;
                kernel.at<float>(0, 5) = 54.0f;
                kernel.at<float>(0, 6) = 39.0f;
                kernel.at<float>(0, 7) = 14.0f;
                kernel.at<float>(0, 8) = -21.0f;
                kernel /= 231.0f;
                return kernel;
            case 11:
                kernel = Mat::zeros(window_size, 1, CV_32F);
                kernel.at<float>(0, 0) = -36.0f;
                kernel.at<float>(0, 1) = 9.0f;
                kernel.at<float>(0, 2) = 44.0f;
                kernel.at<float>(0, 3) = 69.0f;
                kernel.at<float>(0, 4) = 84.0f;
                kernel.at<float>(0, 5) = 89.0f;
                kernel.at<float>(0, 6) = 84.0f;
                kernel.at<float>(0, 7) = 69.0f;
                kernel.at<float>(0, 8) = 44.0f;
                kernel.at<float>(0, 9) = 9.0f;
                kernel.at<float>(0, 10) = -36.0f;
                kernel /= 429.0f;
                return kernel;
        }
        return kernel;
    }

    cv::Mat DrivableSeg::GetUniformKernel(int window_size, int type) const {
        using cv::Mat;
        if (window_size % 2 == 0) {
            throw std::logic_error("only odd window size allowed");
        }
        cv::Mat kernel = Mat::zeros(window_size, 1, type);
        kernel.at<float>(0, 0) = 1;
        kernel.at<float>(window_size - 1, 0) = 1;
        kernel /= 2;
        return kernel;
    }

    void DrivableSeg::Filter(cv::Mat &angle_img) {
        // todo 选择合适的计算方法
        float dyn_coff;

        for (int c = 0; c < _Horizon_SCAN; c++)
            for (int r = _N_SCAN - 2; r >= _ScanEndRow; r--) {

                dyn_coff = 1 / (1 + std::exp(_Coefalpha * (angle_img.at<float>(r, c) - angle_img.at<float>(r + 1, c))));
                angle_img.at<float>(r, c) =
                        dyn_coff * angle_img.at<float>(r, c) + (1 - dyn_coff) * angle_img.at<float>(r + 1, c);

                /*angle_img.at<float>(r,c) = (1 - alpha) * angle_img.at<float>(r,c) + alpha * angle_img.at<float>(r+1,c);
                angle_img.at<float>(r,c) = 0.4 * angle_img.at<float>(r,c) + 0.3 * angle_img.at<float>(r-1,c) + 0.3 * angle_img.at<float>(r-2,c);
                dyn_coff = std::exp(((angle_img.at<float>(r,c) - angle_img.at<float>(r+1,c)) * (angle_img.at<float>(r,c) - angle_img.at<float>(r+1,c))+alpha) / alpha);
                angle_img.at<float>(r,c) = (1 - dyn_coff) * angle_img.at<float>(r+1,c) + dyn_coff * angle_img.at<float>(r,c);

                dyn_coff = (angle_img.at<float>(r,c) - angle_img.at<float>(r+1,c))/(fabs(angle_img.at<float>(r,c) - angle_img.at<float>(r+1,c)) +alpha);
                dyn_coff2 = alpha / (fabs(angle_img.at<float>(r,c) - angle_img.at<float>(r+1,c)) +alpha);
                angle_img.at<float>(r,c) = dyn_coff2 * angle_img.at<float>(r-1,c) + dyn_coff2 * angle_img.at<float>(r,c);

                dyn_coff = 1 / (1 + std::exp(alpha * (angle_img.at<float>(r,c) - angle_img.at<float>(r+1,c))));
                angle_img.at<float>(r,c) = dyn_coff * angle_img.at<float>(r,c) + (1- dyn_coff) * angle_img.at<float>(r+1,c);

                dyn_coff = 1 / (1 + std::exp(alpha * (copy_mat.at<float>(r,c) - copy_mat.at<float>(r+1,c))));
                angle_img.at<float>(r,c) = dyn_coff * (copy_mat.at<float>(r,c) - copy_mat.at<float>(r+1,c)) + (1- dyn_coff) * angle_img.at<float>(r+1,c);

                dyn_coff = 1 / (1 + std::exp(alpha * (copy_mat.at<float>(r,c) - copy_mat.at<float>(r+1,c))));
                angle_img.at<float>(r,c) = dyn_coff * (copy_mat.at<float>(r,c) - copy_mat.at<float>(r+1,c)) + angle_img.at<float>(r+1,c);

                if(r == _N_SCAN - 1)
                    angle_img.at<float>(r,c) = alpha * angle_img.at<float>(r-1,c) + (1-alpha) * angle_img.at<float>(r,c);
                else
                    angle_img.at<float>(r,c) = alpha * angle_img.at<float>(r+1,c) + (1-alpha) * angle_img.at<float>(r,c);// + 0.6 * angle_img.at<float>(r,c);*/
            }
    }

    void DrivableSeg::extract_object() {

        SegObjectCloud->clear();
        groundMat.copyTo(labelMat);

        labelCount = 2;//可行驶区域的label为1
        for (size_t i = 0; i < _N_SCAN; ++i)
            for (size_t j = 0; j < _Horizon_SCAN; ++j)
                if (labelMat.at<int8_t>(i, j) == 0)//地面和没有深度的点的label是-1，其余是物体，对其进行标记
                    labelComponents(i, j);


        for (size_t i = 0; i < _N_SCAN; ++i) {
            for (size_t j = 0; j < _Horizon_SCAN; ++j) {
                //不是地面点，不是聚类失败的点，距离雷达小于10的点
                if (labelMat.at<int8_t>(i, j) > 1 && labelMat.at<int>(i, j) != 999999 &&
                    rangeMat.at<float>(i, j) < 15) {//labelMat 里1是地面，>=2 是聚类
                    SegObjectCloud->push_back(ProjectedPointCloud->points[j + i * _Horizon_SCAN]);
                    SegObjectCloud->points.back().intensity = labelMat.at<int8_t>(i, j);//这个应该是设置分割后在rviz中显示的颜色的
                }
            }
        }
    }

    void DrivableSeg::labelComponents(int row, int col) {
        // use std::queue std::vector std::deque will slow the program down greatly
        float d1, d2, alpha, angle;
        int fromIndX, fromIndY, thisIndX, thisIndY;
        bool lineCountFlag[_N_SCAN] = {false};

        queueIndX[0] = row;//这个应该是待标记的队列
        queueIndY[0] = col;
        int queueSize = 1;
        int queueStartInd = 0;
        int queueEndInd = 1;

        allPushedIndX[0] = row;//这个应该是已经被标记了的队列
        allPushedIndY[0] = col;
        int allPushedIndSize = 1;
        //采用的BFS算法
        while (queueSize > 0) {
            // Pop point
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            --queueSize;
            ++queueStartInd;
            // Mark popped point
            labelMat.at<int8_t>(fromIndX, fromIndY) = labelCount;
            // Loop through all the neighboring grids of popped grid
            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) {
                // new index
                thisIndX = fromIndX + (*iter).first;
                thisIndY = fromIndY + (*iter).second;
                // index should be within the boundary
                if (thisIndX < 0 || thisIndX >= _N_SCAN)//X是纵向的
                    continue;
                // at range image margin (left or right side)
                if (thisIndY < 0)
                    thisIndY = _Horizon_SCAN - 1;
                if (thisIndY >= _Horizon_SCAN)
                    thisIndY = 0;
                // prevent infinite loop (caused by put already examined point back)
                if (labelMat.at<int8_t>(thisIndX, thisIndY) != 0)//已经被标记过
                    continue;

                d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY),
                              rangeMat.at<float>(thisIndX, thisIndY));
                d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY),
                              rangeMat.at<float>(thisIndX, thisIndY));

                if ((*iter).first == 0)//如果是左右相邻的点
                    alpha = _segmentAlphaX;
                else//如果是上下相邻的点
                    alpha = _segmentAlphaY;
                //这里角度的计算看不太明白
                angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha)));

                if (angle > _segmentTheta){
                    queueIndX[queueEndInd] = thisIndX;
                    queueIndY[queueEndInd] = thisIndY;
                    ++queueSize;
                    ++queueEndInd;

                    labelMat.at<int8_t>(thisIndX, thisIndY) = labelCount;
                    lineCountFlag[thisIndX] = true;

                    allPushedIndX[allPushedIndSize] = thisIndX;
                    allPushedIndY[allPushedIndSize] = thisIndY;
                    ++allPushedIndSize;
                }
            }
        }

        // check if this segment is valid
        bool feasibleSegment = false;
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
    else if (allPushedIndSize >= _segmentValidPointNum){
            int lineCount = 0;
            for (size_t i = 0; i < _N_SCAN; ++i)
                if (lineCountFlag[i] == true)
                    ++lineCount;
        if (lineCount >= _segmentValidLineNum)
                feasibleSegment = true;
        }
        // segment is valid, mark these points
        if (feasibleSegment == true) {
            ++labelCount;
        } else { // segment is invalid, mark these points
            for (size_t i = 0; i < allPushedIndSize; ++i) {
                labelMat.at<int8_t>(allPushedIndX[i], allPushedIndY[i]) = -1;
            }
        }
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr DrivableSeg::GetSegObjCloud() {
        return SegObjectCloud;
    }

    void DrivableSeg::lidarCallback(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg){

        copyPointCloud(laserCloudMsg);

        ComputeDrivableArea(fullPointCloud,GroundCloud);

        PublishCloud();
    }

    void DrivableSeg::copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

        cloudHeader = laserCloudMsg->header;

//#ifdef zvision
//        laserCloudMsg->fields[3] = "intensity";//zvision 的第三个参数是rgba
//#endif

        pcl::fromROSMsg(*laserCloudMsg, *fullPointCloud);

        // Remove Nan points
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*fullPointCloud, *fullPointCloud, indices);

        int Cloudsize = fullPointCloud->points.size();

        //zvision reverse
        for(size_t i = 0;i<Cloudsize;i++){
            fullPointCloud->points[i].x = -fullPointCloud->points[i].x;
            fullPointCloud->points[i].z = -fullPointCloud->points[i].z;
        }
        //只要20*20以内的点云
//        std::vector<int> boxIndices_boundary;
//        boundary_box_filter_.setInputCloud(fullPointCloud);
//        boundary_box_filter_.filter(boxIndices_boundary);//
//        boundary_box_filter_.filter(*fullPointCloud);

        //体素滤波下采样
//        Voxel_box_filter_.setInputCloud(fullPointCloud);
//        Voxel_box_filter_.filter(*fullPointCloud);

//        Outlier_box_filter_.setInputCloud(fullPointCloud);
//        Outlier_box_filter_.filter(*fullPointCloud);
    }

    void DrivableSeg::PublishCloud(){

        sensor_msgs::PointCloud2 laserCloudTemp;

        if (pub_ground.getNumSubscribers() != 0){
            pcl::toROSMsg(*GroundCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            pub_ground.publish(laserCloudTemp);
        }

        if (pub_segmented_obj.getNumSubscribers() != 0){
            pcl::toROSMsg(*SegObjectCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            pub_segmented_obj.publish(laserCloudTemp);
        }
        if (pub_reversed_zvision_lidar.getNumSubscribers() != 0){
            pcl::toROSMsg(*ProjectedPointCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            pub_reversed_zvision_lidar.publish(laserCloudTemp);
        }
    }

    void DrivableSeg::gpf_label_drivabel_area(pcl::PointCloud<pcl::PointXYZI>::Ptr& g_ground_pc){

        gpfmethod->setInputCloud(fullPointCloud);
        gpfmethod->filterGround(g_ground_pc,normal_out,th_dist_d_out);

        Eigen::MatrixXf points(fullPointCloud->points.size(),3);

        Eigen::Vector3d result;
        int j = 0;
        for(size_t row = 0;row<_N_SCAN;row++)
            for(size_t col = 0;col<_Horizon_SCAN;col++){
                points.row(j)<<fullPointCloud->points[row*_Horizon_SCAN+col].x;
                points.row(j)<<fullPointCloud->points[row*_Horizon_SCAN+col].y;
                points.row(j++)<<fullPointCloud->points[row*_Horizon_SCAN+col].z;
            }

//                result = fullPointCloud->points[row*_Horizon_SCAN+col]*normal_out;
//                if(result<th_dist_d_out){
//                    g_ground_pc->points.push_back(fullPointCloud->points[row*_Horizon_SCAN+col]);
//
//                }
//
//            }


//        for(auto p:fullPointCloud->points){
//            points.row(j++)<<p.x,p.y,p.z;
//        }
//        // ground plane model
//        Eigen::VectorXf result = points*normal_out;
//        // threshold filter
//        for(int r=0;r<result.rows();r++){
//            if(result[r]<th_dist_d_out){
//                g_ground_pc->points.push_back(fullPointCloud->points[r]);
//
//        }
//
    }

}

